clc;
clear;
close all;

I=5e-3;
B=2e-2;
Gc=0.49;
G=0.49*0.9;


T=0.0001;tp=2;Kp=22.56;Kv=9.5;Ki=70;
theta_d=zeros(1,1/T+1);
dtheta_d=zeros(1,1/T+1);
ddtheta_d=zeros(1,1/T+1);
theta=zeros(1,1/T+1);
dtheta=zeros(1,1/T+1);
ddtheta=zeros(1,1/T+1);
theta_start=5;
theta_end=0;
i=1;
E=0;dE=0;fE=0;theta_latest=theta_start;dtheta_latest=0;
for t=0:T:tp
    [theta_d(1,i),dtheta_d(1,i),ddtheta_d(1,i)]=cri_damping(t,theta_start,theta_end);
    E=theta_d(1,i)-theta_latest;
    dE=dtheta_d(1,i)-dtheta_latest;
    fE=fE+E*T;
    tor_c=I*(ddtheta_d(1,i)+Kp*E+Kv*dE+Ki*fE)+B*dtheta_latest+Gc*cos(theta_latest); % 正常PD不带I，PID的话带I
    ddtheta(1,i)=(tor_c-(B*dtheta_latest+G*cos(theta_latest)))/I; % 正常用Gc，带质量误差之后用G
    dtheta(1,i)=dtheta_latest+ddtheta(1,i)*T;
    theta(1,i)=theta_latest+dtheta(1,i)*T;
    theta_latest=theta(1,i);
    dtheta_latest=dtheta(1,i);
    i=i+1;
end

t=0:T:tp;
figure(1)
plot(t,theta)
hold on
plot(t,theta_d)
hold on
plot(t,theta_d-theta)
legend("实际值","期望值","误差")
title("角度")
figure(2)
plot(t,dtheta_d-dtheta)
title("角速度误差")
figure(3)
plot(t,ddtheta_d-ddtheta)
title("角加速度误差")

